#include <iostream>
#include <vector>
#include <cmath>
#include <Eigen/Dense>

using Eigen::Vector2d;
using std::vector;

// Helper function to compute the intersection of two lines
std::pair<bool, Vector2d> line_intersection(const Vector2d& A, const Vector2d& B, const Vector2d& C, const Vector2d& D) {
    // Line AB represented as a1x + b1y = c1
    double a1 = B[1] - A[1];
    double b1 = A[0] - B[0];
    double c1 = a1 * (A[0]) + b1 * (A[1]);
  
    // Line CD represented as a2x + b2y = c2
    double a2 = D[1] - C[1];
    double b2 = C[0] - D[0];
    double c2 = a2 * (C[0]) + b2 * (C[1]);
  
    double determinant = a1 * b2 - a2 * b1;
  
    if (determinant == 0) {
        // The lines are parallel. This is simplified
        // code for readability. You may want to handle this case differently.
        return std::make_pair(false, Vector2d(0, 0));
    } else {
        double x = (b2 * c1 - b1 * c2) / determinant;
        double y = (a1 * c2 - a2 * c1) / determinant;
        return std::make_pair(true, Vector2d(x, y));
    }
}

// Function to calculate all tangent lines from given centers and radius
vector<Vector2d> calculate_all_tangent_lines(const vector<Vector2d>& centers, double radius) {
    vector<Vector2d> intersection_points;

    for (size_t i = 1; i < centers.size() - 1; ++i) {
        auto [x1, y1] = centers[i - 1];
        auto [x2, y2] = centers[i];
        auto [x3, y3] = centers[i + 1];

        // Compute angles using atan2
        double angle1 = std::atan2(y2 - y1, x2 - x1);
        double angle2 = std::atan2(y3 - y2, x3 - x2);

        // Compute tangent points
        Vector2d t1_up = {x1 + radius * std::sin(angle1), y1 - radius * std::cos(angle1)};
        Vector2d t2_up = {x2 + radius * std::sin(angle1), y2 - radius * std::cos(angle1)};
        Vector2d t1_down = {x1 - radius * std::sin(angle1), y1 + radius * std::cos(angle1)};
        Vector2d t2_down = {x2 - radius * std::sin(angle1), y2 + radius * std::cos(angle1)};

        Vector2d t3_up = {x2 + radius * std::sin(angle2), y2 - radius * std::cos(angle2)};
        Vector2d t4_up = {x3 + radius * std::sin(angle2), y3 - radius * std::cos(angle2)};
        Vector2d t3_down = {x2 - radius * std::sin(angle2), y2 + radius * std::cos(angle2)};
        Vector2d t4_down = {x3 - radius * std::sin(angle2), y3 + radius * std::cos(angle2)};

        // Check if lines are parallel
        auto [intersect_up, point_up] = line_intersection(t1_up, t2_up, t3_up, t4_up);
        if (intersect_up) {
            intersection_points.push_back(point_up);
        }

        auto [intersect_down, point_down] = line_intersection(t1_down, t2_down, t3_down, t4_down);
        if (intersect_down) {
            intersection_points.push_back(point_down);
        }
    }

    return intersection_points;
}

int main() {
    // Define your data or load from somewhere
    vector<Vector2d> data = {{-4.957085, 2.705975}, {-4.907085, 2.705719}, /* Add more points as needed */};

    // Calculate tangent lines
    auto intersection_points = calculate_all_tangent_lines(data, 1.0);

    // Output intersection points (example)
    for (const auto& pt : intersection_points) {
        std::cout << "Intersection Point: " << pt[0] << ", " << pt[1] << std::endl;
    }

    return 0;
}
